#ifndef BARREL_DETECTOR_HPP
#define BARREL_DETECTOR_HPP

#include "detector_base.hpp"
#include <geometry_msgs/msg/point_stamped.hpp>
#include <pcl/common/centroid.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <rclcpp/publisher.hpp>


namespace mult_lidar{

    struct BarrelDetectionParams
    {
        float ring_height_threshold = 0.2f;
        float barrel_radius = 0.2f;
        float radius_filter_margin = 0.1f;
        float search_start_z = 0.6f;
        float search_end_z = 0.4f;
        float search_step = 0.01f;
        int min_points_in_slice = 30;
        int min_points_after_filter = 10;
        float max_radius_error = 0.1f;

    };

    class BarrelDetector : public DetectorBase {

    public:

        BarrelDetector();
        ~BarrelDetector() = default;

        bool detectCenter(PointCloud cloud, geometry_msgs::msg::PointStamped& center_point) override;

        //void setParameters(const std::shared_ptr<rclcpp::Node>& node) override;
        void setParameters(rclcpp::Node* node) override;

        void setRingCloudPublisher(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub){
            ring_cloud_pub_ = pub;
        }


    private:
        
        BarrelDetectionParams params_;
        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr ring_cloud_pub_;

        PointCloud extractRingCloud(const PointCloud& cloud, float& detected_z);

        PointCloud filterByRadius(const PointCloud& ring_cloud);

        bool fitCircle(const PointCloud& cloud, float& center_x, float& center_y);

        bool validateFit(const PointCloud& cloud, float center_x, float center_y);

    };
    
}

#endif